import rclpy
from rclpy.node import Node
from rclpy.action import ActionServer, ActionClient
from inters.srv import TargetGet, CarInfoGet
from inters.action import Drive, Move, Turn
import sys
import getopt
import os
from ament_index_python.packages import get_package_share_directory


def run_as_debug() -> bool:
    """
    是否传递了参数debug
    """
    debug = False
    # 获取文件名（含后缀）
    name = os.path.basename(__file__)
    try:
        """
        options, args = getopt.getopt(args, shortopts, longopts=[])

        参数args：一般是sys.argv[1:]。过滤掉sys.argv[0]，它是执行脚本的名字，不算做命令行参数。
        参数shortopts：短格式分析串。例如："hp:i:"，h后面没有冒号，表示后面不带参数；p和i后面带有冒号，表示后面带参数。
        参数longopts：长格式分析串列表。例如：["help", "ip=", "port="]，help后面没有等号，表示后面不带参数；ip和port后面带等号，表示后面带参数。

        返回值options是以元组为元素的列表，每个元组的形式为：(选项串, 附加参数)，如：('-i', '192.168.0.1')
        返回值args是个列表，其中的元素是那些不含'-'或'--'的参数。
        """
        opts, args = getopt.getopt(sys.argv[1:], "hd:", ["help", "debug="])
        # 处理 返回值options是以元组为元素的列表。
        for opt, arg in opts:
            if opt in ("-h", "--help"):
                print(f"{name} -d <True|False>")
                print(f"or: {name} --debug=<True|False>")
                sys.exit()
            elif opt in ("-d", "--debug"):
                debug = bool(arg)
        print("debug为：", debug)

        # 打印 返回值args列表，即其中的元素是那些不含'-'或'--'的参数。
        for i in range(0, len(args)):
            print("参数 %s 为：%s" % (i + 1, args[i]))
    except getopt.GetoptError:
        print(f"Error: {name} -d <True|False>")
        print(f"   or: {name} --debug=<True|False>")
    return debug


if run_as_debug():
    from brain_part.expert_system import ExpertSystem
    from brain_part.neural_network import NeuralNetwork
else:
    from kitt.brain_part.expert_system import ExpertSystem
    from kitt.brain_part.neural_network import NeuralNetwork


class Brain(Node):
    def __init__(self, node_name):
        super().__init__(node_name=node_name)
        if run_as_debug():
            self.share_path = os.path.join(os.path.dirname(__file__), "../resource")
        else:
            self.share_path = get_package_share_directory("kitt")
        self.car_world_x = float(0)
        self.car_world_y = float(0)
        self.car_world_angle = float(0)
        self.has_target = False
        self.target_world_x = float(0)
        self.target_world_y = float(0)
        self._car_info_get_client = self.create_client(CarInfoGet, "/car/info/get")
        self._target_get_client = self.create_client(TargetGet, "/car/target/get")
        self._turn_action_client = ActionClient(self, Turn, "/car/turn")
        self._move_action_client = ActionClient(self, Move, "/car/move")
        self._drive_action_server = ActionServer(
            self, Drive, "/brain/drive", self.drive_callback
        )
        self.expert_system = ExpertSystem()
        self.neural_network = NeuralNetwork(self)
        self.turning = False
        self.moving = False

    async def drive_callback(self, goal_handle):
        self.get_logger().info("Executing drive goal...")
        await self.send_car_info_get()
        await self.send_target_get()
        if self.has_target:
            drive_type = goal_handle.request.drive_type
            feedback_msg = Drive.Feedback()

            if drive_type == 0:  # 专家系统
                while True:
                    (
                        done,
                        angle_direction,
                        angle,
                        distance_direction,
                        distance,
                    ) = self.expert_system.create_next_step(
                        self.car_world_angle,
                        self.car_world_x,
                        self.car_world_y,
                        self.target_world_x,
                        self.target_world_y,
                    )
                    if done or distance < 1:
                        break
                    feedback_msg.angle = angle
                    feedback_msg.distance = distance
                    self.get_logger().info(
                        f"/brain/drive Feedback: ({angle:.2f}, {distance:.2f})"
                    )
                    goal_handle.publish_feedback(feedback_msg)
                    if angle != 0:
                        await self.send_car_turn(angle_direction, angle)
                    if distance != 0:
                        await self.send_car_move(distance_direction, distance)
                pass

            if drive_type == 1:  # 神经网络
                while True:
                    (
                        done,
                        angle_direction,
                        angle,
                        distance_direction,
                        distance,
                    ) = self.neural_network.create_next_step(
                        self.car_world_angle,
                        self.car_world_x,
                        self.car_world_y,
                        self.target_world_x,
                        self.target_world_y,
                    )
                    if done or distance < 1:
                        break
                    feedback_msg.angle = angle
                    feedback_msg.distance = distance
                    self.get_logger().info(
                        f"/brain/drive Feedback: ({angle:.2f}, {distance:.2f})"
                    )
                    goal_handle.publish_feedback(feedback_msg)
                    if angle != 0:
                        await self.send_car_turn(angle_direction, angle)
                    if distance != 0:
                        await self.send_car_move(distance_direction, distance)
                pass

        await self.send_car_info_get()
        goal_handle.succeed()
        result = Drive.Result()
        result.world_angle = self.car_world_angle
        result.world_x = self.car_world_x
        result.world_y = self.car_world_y
        return result

    async def send_target_get(self):
        while not self._target_get_client.wait_for_service(1.0):
            self.get_logger().info("service not available, waiting again...")
            return
        req = TargetGet.Request()
        target_get_future = self._target_get_client.call_async(req)
        result = await target_get_future
        self.has_target, self.target_world_x, self.target_world_y = (
            result.has_target,
            result.world_x,
            result.world_y,
        )

    async def send_car_info_get(self):
        while not self._car_info_get_client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info("service not available, waiting again...")
            return
        req = CarInfoGet.Request()
        car_info_get_future = self._car_info_get_client.call_async(req)
        result = await car_info_get_future
        self.car_world_x, self.car_world_y, self.car_world_angle = (
            result.world_x,
            result.world_y,
            result.world_angle,
        )

    async def send_car_turn(self, direction, angle):
        if self.turning:
            return

        self.turning = True
        goal_msg = Turn.Goal()
        goal_msg.direction = direction
        goal_msg.angle = angle
        self._turn_action_client.wait_for_server()
        turn_send_goal_future = self._turn_action_client.send_goal_async(goal_msg)
        await turn_send_goal_future
        goal_handle = turn_send_goal_future.result()
        if not goal_handle.accepted:
            self.get_logger().info("Goal rejected :(")
            self.turning = False
            return

        self.get_logger().info("Goal accepted :)")
        turn_get_result_future = goal_handle.get_result_async()
        await turn_get_result_future
        result = turn_get_result_future.result().result
        self.get_logger().info(f"Result: ({result.angle:.2f},{result.world_angle:.2f})")
        self.car_world_angle = result.world_angle
        self.turning = False

    async def send_car_move(self, direction, distance):
        if self.moving:
            return

        self.moving = True
        goal_msg = Move.Goal()
        goal_msg.direction = direction
        goal_msg.distance = distance
        self._move_action_client.wait_for_server()
        move_send_goal_future = self._move_action_client.send_goal_async(goal_msg)
        await move_send_goal_future
        goal_handle = move_send_goal_future.result()
        if not goal_handle.accepted:
            self.get_logger().info("Goal rejected :(")
            self.moving = False
            return

        self.get_logger().info("Goal accepted :)")
        move_get_result_future = goal_handle.get_result_async()
        await move_get_result_future
        result = move_get_result_future.result().result
        self.get_logger().info(
            f"Result: ({result.distance:.2f},{result.world_x:.2f},{result.world_y:.2f})"
        )
        self.car_world_x, self.car_world_y = (result.world_x, result.world_y)
        self.moving = False


def main(args=None):
    rclpy.init(args=args)
    brain = Brain("Brain")
    rclpy.spin(brain)
    brain.destroy_node()
    rclpy.shutdown()


if __name__ == "__main__":
    # sys.argv[1:]为要处理的参数列表，sys.argv[0]为脚本名，所以用sys.argv[1:]过滤掉脚本名。
    main(sys.argv[1:])
